#include "xfultrasonicdriver/XfParkSensorDriver.h"

XfParkSensorDriver::XfParkSensorDriver(ros::NodeHandle &m_handle)
{ // 构造函数初始化UDP端口

  ros::NodeHandle private_nh("~");

  m_handle.param<bool>("bdebug", bdebug, false);

  // connect
  m_canetip = "192.168.1.178"; // CANet的通信IP
  m_canetport = 4002;          // CANet端口号
  m_pcip = "192.168.1.4";      // 主机通信IPS
  m_pcport = 4002;             // 主机端口号

  memset(&ultrasonic_info, 0xFE, sizeof(ultrasonic_info));

  // paramters
  std::cout << FYEL("*****ParkSensor can parameters**************")
            << std::endl;
  std::cout << FGRN("canet_ip: ") << m_canetip << std::endl;
  std::cout << FGRN("canet_port: ") << m_canetport << std::endl;
  std::cout << FGRN("pc_ip: ") << m_pcip << std::endl;
  std::cout << FGRN("pc_port: ") << m_pcport << std::endl;
  std::cout << FYEL("*****ParkSensor can parameters end**********") << std::endl;

  // io
  boost::asio::io_service io_service;
  boost_udp =
      new Boost_UDP(io_service, m_pcip, m_pcport, m_canetip, m_canetport);
  boost_udp->start_sock();

  pub_ultrasonic = m_handle.advertise<xfultrasonicdriver::ultrasonic_msg>("/psd_ultrasonic", 1); // 发布节点

  ROS_INFO_STREAM("XfParkSensorDriver Init OK!"); // Prompt message //提示信息
}

XfParkSensorDriver::~XfParkSensorDriver() // 析构函数，在停止对象调用时调用
{
  delete boost_udp; // 释放UDP调用内存
}

void XfParkSensorDriver::run()
{

  ros::Rate rate(100); // 100Hz
  int count = 0;
  while (ros::ok())
  {
    // ros::spinOnce();

    if ((count & 0x01) == 0)
    {
      count++;
      // SendEnableRequestKernel();
    }
    RecvCarInfoKernel();

    rate.sleep();
  }
}
// 发送指令
void XfParkSensorDriver::SendEnableRequestKernel()
{

  unsigned char send_buf[13] = {0, 0};
  //  cout<<"aim_speed_l: "<<speed_l<<"aim_speed_r: "<<speed_r<<endl;

  // 发送
  memset(&send_buf, 0, sizeof(send_buf));
  send_buf[0] = 0x08; // 标准帧
  send_buf[1] = 0x00;
  send_buf[2] = 0x00;
  send_buf[3] = 0x05; // id:0x562
  send_buf[4] = 0x62;

  // 前雷达工作请求（2~7#雷达）
  // FICMFrontPDCEnableRequestvalidity
  // 后雷达工作请求（10~15#雷达）
  // FICMRearPDCEnableRequestvalidity
  send_buf[5] = 0x0F; // 全部使能
  // send_buf[6] = 0x00;
  // send_buf[7] = 0x00;
  // send_buf[8] = 0x00;
  // send_buf[9] = 0x00;
  // send_buf[10] = 0x00;
  // send_buf[11] = 0x00;
  // send_buf[12] = 0x00;
  int ret = boost_udp->send_data(send_buf, 13); // 通过UDP发送数据
}

void XfParkSensorDriver::RecvCarInfoKernel()
{ // 接受CANet的数据

  memset(buffer, 0, sizeof(buffer));         // 缓存buffer清零
  int ret = boost_udp->receive_data(buffer); // ret表示的是数据长度的字节数
  // cout<<"ret : " << ret <<endl;

  if (ret > 0) // 13　　接收13的倍数个字节的数据
  {
    ParserData(buffer, ret); // 一次收到的有多帧数据,我们需要解析。
                             // 边收边解析，每接受到13个字节的数据会进行一次解析
    // cout<<"len : " << ret <<endl;
  }
}

void XfParkSensorDriver::ParserData(unsigned char data[], int num) // buffer是数据缓存数组，num是数据接收的字节数．
{
  bool received = false;

  for (int i = 0; i < num / 13; ++i) // 从第1３个字节以上
  {
    stCANMsg frame; // 接收到的数据报文信息，包头＋ID + 数据
    CHAR2UINT ii;

    for (int j = 0; j < 4; ++j)
    {
      ii.ch[3 - j] = data[1 + j + i * 13];
    }
    unsigned int id = ii.i;
    frame.ID = id;
    for (int j = 0; j < 8; ++j)
    {
      frame.data[j] = data[5 + j + i * 13];
    }

    if (frame.ID == 0x28a)
    {
      received = true;
      ultrasonic_info.psd01 = frame.data[0];
      ultrasonic_info.psd02 = frame.data[1];
      ultrasonic_info.psd03 = frame.data[2];
      ultrasonic_info.psd04 = frame.data[3];
      ultrasonic_info.psd05 = frame.data[4];
      ultrasonic_info.psd06 = frame.data[5];
      if (bdebug)
      {

        ROS_INFO("%X: %02x %02x %02x %02x %02x %02x %02x %02x ", frame.ID, frame.data[0], frame.data[1], frame.data[2], frame.data[3], frame.data[4], frame.data[5], frame.data[6], frame.data[7]);
      }
    }
    else if (frame.ID == 0x289)
    {
      received = true;
      ultrasonic_info.psd07 = frame.data[0];
      ultrasonic_info.psd08 = frame.data[1];
      ultrasonic_info.psd09 = frame.data[2];
      ultrasonic_info.psd10 = frame.data[3];
      ultrasonic_info.psd11 = frame.data[4];
      ultrasonic_info.psd12 = frame.data[5];
    }
  }
  if (received)
  {
    pub_ultrasonic.publish(ultrasonic_info);
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "xfparksensordriver");
  ros::NodeHandle nh;
  XfParkSensorDriver Renode(nh);
  Renode.run();
  return 0;
}
